/*
deal_cmd_ac.c
offer deal with recive cmd from upper
designed by lunanting

----------------------------
2020-06-29 10:33:35
create file
----------------------------
*/

#include "deal_cmd_ac.h"
#include "../stringdef.h"
#include "string.h"
#include "../../driver/pansonic/wheel.h"
#include "../../driver/gyro/gyro.h"
#include "stdlib.h"
#include "../upper.h"
#include "lidar_navigation.h"

static int speed = 217;	//默认0.1m/s

int32_t agv_run_fun(float run_speed)
{
	int32_t ret = 0;

	speed = xp_wheel_get_speed(run_speed);
	println("agv_run_fun: run_speed %f, speed %d", run_speed, speed);
	ret = xp_wheel_motorSpeed_set(speed,speed);
	if(0 == ret){
		printf("agv_run is success\n");
		return 0;
	}else{
		printf("agv_run is err,err value is %d\n", ret);
		return -1;
	}
}

int32_t agv_stop_fun()
{
    int32_t ret = -1;

    ret = xp_wheel_motorSpeed_set(0, 0);
    if(0 == ret){
		println("agv_stop is success\n");
		return 0;
	}else{
		println("agv_stop is err,err value is %d\n", ret);
		return -1;
	}
}

static int32_t agv_by_car_fun(const char *value)
{
	xp_upper_send("ok");
	set_round_test_end_flag(false);
	xp_robot_round_test();
}

static int32_t agv_by_car_end_fun(const char *value)
{
	xp_upper_send("ok");
	set_round_test_end_flag(true);
}

int32_t agv_car_going_fun(int8_t side)
{
	xp_set_robot_round_side(side);
	xp_round_side_signal();

	return 0;
}

int32_t agv_is_car_going_end_fun()
{
	if(xp_is_robot_round_side_end()){
		return 1;
	}else{
		return 0;
	}
}

int32_t agv_is_robot_fix_angle_end_fun()
{
	if(xp_is_robot_straightening_end()){
		return 1;
	}else{
		return 0;
	}
}

int32_t agv_car_turning_fun(int8_t type)
{
	xp_set_robot_turning_type(type);
	xp_turning_signal();

	return 0;
}

int32_t agv_is_car_turning_end_fun()
{
	if(xp_is_robot_turning_end()){
		return 1;
	}else{
		return 0;
	}
}

int32_t agv_car_turn_to_side(int8_t side)
{
	int32_t ret = -1;

	ret = xp_robot_turn_to_side(side);

	return ret;
}

int32_t agv_is_car_leave_finish_fun()
{
	if(xp_car_leave_state()){
		return 1;
	}else{
		return 0;
	}
}

int32_t agv_is_round_side_leave_car_fun()
{
	if(xp_is_round_side_leave_car()){
		return 1;
	}else{
		return 0;
	}
}

int32_t agv_is_car_search_finish_fun()
{
	if(xp_is_robot_search_car()){
		return 1;
	}else{
		return 0;
	}
}

int32_t agv_car_safe_distance_fun(int32_t dist)
{
	println("agv_car_safe_distance_fun: dist %d", dist);
	xp_set_robot_safe_distance(dist);
	return 0;
}

int32_t agv_car_round_side_time_reset_gyro_fun(int32_t time)
{
	set_round_side_reset_gyro_timeout(time);
	xp_reset_robot_round_side_reset_gyro();
	xp_round_side_reset_gyro_signal();

	return 0;
}

int32_t agv_car_round_side_dist_reset_gyro_fun(int32_t dist)
{
	set_round_side_reset_gyro_dist(dist);
	xp_reset_robot_round_side_reset_gyro();
	xp_reset_robot_dist_reset_gyro_round_side_end();
	xp_round_side_dist_reset_gyro_signal();

	return 0;
}

int32_t agv_is_car_round_side_reset_gyro_end_fun()
{
	if(xp_is_robot_round_side_reset_gyro_end()){
		return 1;
	}else{
		return 0;
	}
}

int32_t agv_is_car_dist_reset_gyro_round_side_end_fun()
{
	if(xp_is_robot_dist_reset_gyro_round_side_end()){
		return 1;
	}else{
		return 0;
	}
}

int32_t agv_car_adjust_pose_by_lidar_fun(int32_t time)
{
	set_adjust_pose_by_lidar_timeout(time);
	xp_reset_robot_adjust_pose_by_lidar();
	xp_adjust_pose_by_lidar_signal();

	return 0;
}

int32_t agv_leave_adjust_pose_by_lidar_fun(int32_t time)
{
	set_adjust_pose_by_lidar_timeout(time);
	xp_reset_robot_adjust_pose_by_lidar();
	xp_leave_adjust_pose_by_lidar_signal();

	return 0;
}

int32_t agv_car_is_adjust_pose_end()
{
	if(xp_is_robot_adjust_pose_by_lidar_end()){
		return 1;
	}else{
		return 0;
	}
}

static int32_t agv_turn_left_fun(const char *value)
{
	xp_upper_send("ok");
	xp_wheel_motorSpeed_set(speed/2,speed);
}

static int32_t agv_turn_right_fun(const char *value)
{
	xp_upper_send("ok");
	xp_wheel_motorSpeed_set(speed,speed/2);
}

static int32_t agv_turn_stop_fun(const char *value)
{
	xp_upper_send("ok");
	xp_wheel_motorSpeed_set(0,0);
}

static int32_t agv_round_left_fun(const char *value)
{
	float f;
	
	xp_upper_send("ok");
	f=atof(value);	
	xp_wheel_motorSpeed_set(0-speed,speed);
}

static int32_t agv_round_right_fun(const char *value)
{
	float f;
	
	xp_upper_send("ok");
	f=atof(value);
	xp_wheel_motorSpeed_set(speed,0-speed);
}

static int32_t agv_round_stop_fun(const char *value)
{	
	xp_upper_send("ok");
	xp_wheel_motorSpeed_set(0,0);
}

int32_t gyro_reset_fun(int8_t side)
{
#ifdef WASHER_MERGE
	robot_angle_soft_reset(side%4);
#else
	xp_gyro_safe_reset();
#endif
	//启动里程计
	xp_odom_start_signal();
}

int32_t agv_set_robot_base_speed_fun(float base_speed)
{
	println("agv_set_robot_base_speed_fun: base_speed %f", base_speed);
	if ((base_speed - 0.35) > 0){
		base_speed = 0.1;//速度值异常则使用默认值0.1m/s
	}else if((base_speed + 0.35) < 0){
		base_speed = -0.1;//速度值异常则使用默认值-0.1m/s
	}
	set_robot_base_speed(base_speed);

	return 0;
}

int32_t agv_robot_right_angle_fun(int32_t side)
{
	set_robot_straightening_side(side);
	xp_reset_robot_straightening_end();
	xp_robot_right_angle_signal();

	return 0;
}

static int32_t agv_enable_set_fun(const char *value)
{		
	int32_t state;

	xp_upper_send("ok");
	state=atol(value);
	xp_wheel_enable_set(state);
}

int32_t agv_set_wash_car_roof_flag_fun(int32_t state)
{
	set_wash_car_roof_flag(state);
	printf("agv_set_wash_car_roof_flag_fun receive state %d\n", state);

	return 0;
}

int32_t agv_set_soft_reset_gyro_flag_fun(int32_t state)
{
	set_soft_reset_gyro_flag(state);
	printf("agv_set_soft_reset_gyro_flag_fun receive state %d\n", state);

	return 0;
}

int32_t agv_set_dist_filter_flag_fun(int32_t state)
{
	static int32_t flag_state;

	set_dist_filter_flag(state);

	if(state != flag_state){
		printf("agv_set_dist_filter_flag_fun receive state %d\n", state);
	}

	flag_state = state;

	return 0;
}

int32_t agv_leave_car_fun(const char *value)
{
	if (0 == strcmp(value, "A") || \
		0 == strcmp(value, "B") || \
		0 == strcmp(value, "C") || \
		0 == strcmp(value, "D")){
			
		xp_leave_sem_set(value[0]);
		return 0;
	}else{
		return -1;
	}
}

//AGV����������
static agv_cmd_replay_st agv_cmd_replay[] = {
	//{UPPER_AGV_RUN, agv_run_fun},
	//{UPPER_AGV_STOP, agv_stop_fun},
	{UPPER_AGV_BY_CAR, agv_by_car_fun},
	{UPPER_AGV_BY_CAR_END, agv_by_car_end_fun},
	//{UPPER_AGV_CAR_GOING, agv_car_going_fun},
	//{UPPER_AGV_IS_CAR_GOING_END, agv_is_car_going_end_fun},
	//{UPPER_AGV_CAR_TURNING, agv_car_turning_fun},
	//{UPPER_AGV_IS_CAR_TURNING_END, agv_is_car_turning_end_fun},
	
	//{UPPER_AGV_LEAVE_CAR, agv_leave_car_fun},
	//{UPPER_AGV_IS_CAR_LEAVE_FINISH, agv_is_car_leave_finish_fun},
	
	//{UPPER_AGV_IS_CAR_SEARCH_FINISH, agv_is_car_search_finish_fun},
	//{UPPER_AGV_CAR_SAFE_DISTANCE, agv_car_safe_distance_fun},
	//{UPPER_AGV_CAR_ROUND_SIDE_TIME_RESET_GYRO, agv_car_round_side_time_reset_gyro_fun},
	//{UPPER_AGV_CAR_IS_ROUND_SIDE_RESET_GYRO_END, agv_is_car_round_side_reset_gyro_end_fun},
	//{UPPER_AGV_CAR_ADJUST_POSE_BY_LIDAR, agv_car_adjust_pose_by_lidar_fun},
	//{UPPER_AGV_CAR_IS_ADJUST_POSE_END, agv_car_is_adjust_pose_end},
	//{UPPER_AGV_CAR_ROUND_SIDE_DIST_RESET_GYRO, agv_car_round_side_dist_reset_gyro_fun},

	{UPPER_AGV_TURN_LEFT, agv_turn_left_fun},
	{UPPER_AGV_TURN_RIGTHT, agv_turn_right_fun},
	{UPPER_AGV_TURN_STOP, agv_turn_stop_fun},
	{UPPER_AGV_ROUND_LEFT, agv_round_left_fun},
	{UPPER_AGV_ROUND_RIGHT, agv_round_right_fun},
	{UPPER_AGV_ROUND_STOP, agv_round_stop_fun},
	//{UPPER_GYRO_RESET, gyro_reset_fun},
	//{UPPER_AGV_BASE_SPEED, agv_set_robot_base_speed_fun},
	
	//{UPPER_AGV_ROBOT_RIGHT_ANGLE, agv_robot_right_angle_fun},
	
	{UPPER_AGV_ENABLE_SET, agv_enable_set_fun},
	
	//{UPPER_AGV_IS_ROBOT_FIX_ANGLE_END, agv_is_robot_fix_angle_end_fun},
	
	//{UPPER_AGV_SET_WASH_CAR_ROOF_FLAG, agv_set_wash_car_roof_flag_fun},
	//{UPPER_AGV_SET_SOFT_RESET_GYRO_FLAG, agv_set_soft_reset_gyro_flag_fun}

};

/*
redefine upper uaer api
param:
    data:recive cmd data
    len:cmd data length
*/	
int xp_upper_user_api(const char *data,u32 len){
    char buf[100]={0};
    char *key,*value,*foot;
    int i=0;
    bool agv_cmd_err = 1;

    strcpy(buf,data);
    key=strtok_r(buf," ",&foot);
    value=foot;
    //println("recive cmd =%s,value=%s",key,value);

	 for (i = 0; i < sizeof(agv_cmd_replay)/sizeof(agv_cmd_replay[0]); i++){
        if (strcmp(key,agv_cmd_replay[i].agv_cmd) == 0){
			//agv�����
            agv_cmd_replay[i].agv_cmd_fun(value);
			agv_cmd_err = 0;
            break;
        }
    }

	if(strcmp(key,"set_wheel")==0){
		float s[2];
		char *p1,*f1;

		agv_cmd_err=0;
		xp_upper_send("ok");
		p1=strtok_r(value,",",&f1);
		s[0]=(float)atof(p1);
		s[1]=(float)atof(f1);
		xp_wheel_set_wheel_speed(s[0],s[1]);
	}
    
	if(agv_cmd_err){
        xp_upper_send("error");
	}

    return 0;
}



/**************************************merge api********************************/






//end of the file
